#include <stdio.h>
#include <string.h>
#include <UserAPI.h>
#include "VEXRobot.h"
#include "Serial_Ports.h"
#include "MyConfig.h"

static char resultBuff[MAX_DATA_BLOCK_SIZE];
static short resultLength;
static char resultCommand;
long interruptValues[NUM_INTERUPTS];
short adcValues[NUM_IO_PORTS];

/*
** Initialize the result buffer
*/
static void InitResult(short length, char command)
{
   // Set the result length and the command executed
   resultLength = length;
   resultCommand = command;

   // Reset the buffer
   memset(resultBuff, 0, length);
}

/*
** Helper functions to set result data
*/
void SetResultByte(int offset, char value)
{
   // Make sure the requested offset is valid
   if(offset < resultLength)
   {
      resultBuff[offset] = value;
   }
#ifdef XMIT_DEBUG
   else
   {
      printf("ERROR - Byte write past EOR\n");
   }
#endif
}
void SetResultShort(int offset, short value)
{
   // Make sure the requested offset is valid
   if(offset + 1 < resultLength)
   {
      *((short *) &resultBuff[offset]) = value;
   }
#ifdef XMIT_DEBUG
   else
   {
      printf("ERROR - Short write past EOR\n");
   }
#endif
}
void SetResultLong(int offset, long value)
{
   // Make sure the requested offset is valid
   if(offset + 3 < resultLength)
   {
      *((long *) &resultBuff[offset]) = value;
   }
#ifdef XMIT_DEBUG
   else
   {
      printf("ERROR - Long write past EOR\n");
   }
#endif
}

/*
* Send a result back to the host
*/
static void SendResult(void)
{
   char  checksum = 0;
   short i;

   // Header format
   // short    preamble
   // byte     packet length (not including preamble)
   // byte     checksum
   // byte     command
   WRITE_PC_PORT('$');  // VT_20070705_PORT
   WRITE_PC_PORT('$');  // VT_20070705_PORT

   // Set the command processed
   WRITE_PC_PORT(resultCommand); // VT_20070705_PORT
#ifdef XMIT_DEBUG
   printf("XMIT Command: %d\n", resultCommand);
#endif

   // Send the data packet length
   WRITE_PC_PORT(resultLength);  // VT_20070705_PORT
#ifdef XMIT_DEBUG
   printf("XMIT Length: %d\n", resultLength);
#endif

   // Calculate the checksum
   checksum ^= resultCommand;
   for(i=0; i<resultLength; ++i)
   {
      checksum ^= resultBuff[i];
   }
   WRITE_PC_PORT(checksum);   // VT_20070705_PORT
#ifdef XMIT_DEBUG
   printf("XMIT Checksum: %d\n", checksum);
#endif

   // Set the data payload
   for(i=0; i<resultLength; ++i)
   {
      WRITE_PC_PORT(resultBuff[i]); // VT_20070705_PORT
#ifdef XMIT_DEBUG
      printf(" %d ", resultBuff[i]);
#endif
   }

   // Reset the data length (just in case)
   resultCommand = 88;
   resultLength = 0;
}

/*
** Check for the connect string
*/
static int ConnectCheck(unsigned char ch)
{
   /*
   ** Use a simple state machine to see if we process the
   ** "CONNECT" message from the modem
   */
   static enum ConnectState
   {
   CONNECT_C,
   CONNECT_O,
   CONNECT_N1,
   CONNECT_N2,
   CONNECT_E,
   CONNECT_C2,
   CONNECT_T
   } connectState = CONNECT_C;

   switch(connectState)
   {
   case CONNECT_C:
      if(ch == 'C')
         connectState = CONNECT_O;
      break;
   case CONNECT_O:
      if(ch == 'O')
         connectState = CONNECT_N1;
      else
         connectState = CONNECT_C;
      break;
   case CONNECT_N1:
      if(ch == 'N')
         connectState = CONNECT_N2;
      else
         connectState = CONNECT_C;
      break;
   case CONNECT_N2:
      if(ch == 'N')
         connectState = CONNECT_E;
      else
         connectState = CONNECT_C;
      break;
   case CONNECT_E:
      if(ch == 'E')
         connectState = CONNECT_C2;
      else
         connectState = CONNECT_C;
      break;
   case CONNECT_C2:
      if(ch == 'C')
         connectState = CONNECT_T;
      else
         connectState = CONNECT_C;
      break;
   case CONNECT_T:
      if(ch == 'T')
      {
         CommunicationsConnect();
         return 1;
      } else
         connectState = CONNECT_C;
      break;
   }
   return 0;
}

//
// Check for the no-carrier string
//
static int NoCarrierCheck(unsigned char ch)
{
   /*
   ** Use a simple state machine to see if we process the
   ** "NO CARRIER" message from the modem
   */
   static enum NoCarrierState
   {
   NO_CARRIER_N,
   NO_CARRIER_O,
   NO_CARRIER_SP,
   NO_CARRIER_C,
   NO_CARRIER_A,
   NO_CARRIER_R1,
   NO_CARRIER_R2,
   NO_CARRIER_I,
   NO_CARRIER_E,
   NO_CARRIER_R3
   } noCarrierState = NO_CARRIER_N;

   switch(noCarrierState)
   {
   case NO_CARRIER_N:
      if(ch == 'N')
         noCarrierState = NO_CARRIER_O;
      else
         noCarrierState = NO_CARRIER_N;
      break;
   case NO_CARRIER_O:
      if(ch == 'O')
         noCarrierState = NO_CARRIER_SP;
      else
         noCarrierState = NO_CARRIER_N;
      break;
   case NO_CARRIER_SP:
      if(ch == ' ')
         noCarrierState = NO_CARRIER_C;
      else
         noCarrierState = NO_CARRIER_N;
      break;
   case NO_CARRIER_C:
      if(ch == 'C')
         noCarrierState = NO_CARRIER_A;
      else
         noCarrierState = NO_CARRIER_N;
      break;
   case NO_CARRIER_A:
      if(ch == 'A')
         noCarrierState = NO_CARRIER_R1;
      else
         noCarrierState = NO_CARRIER_N;
      break;
   case NO_CARRIER_R1:
      if(ch == 'R')
         noCarrierState = NO_CARRIER_R2;
      else
         noCarrierState = NO_CARRIER_N;
      break;
   case NO_CARRIER_R2:
      if(ch == 'R')
         noCarrierState = NO_CARRIER_I;
      else
         noCarrierState = NO_CARRIER_N;
      break;
   case NO_CARRIER_I:
      if(ch == 'I')
         noCarrierState = NO_CARRIER_E;
      else
         noCarrierState = NO_CARRIER_N;
      break;
   case NO_CARRIER_E:
      if(ch == 'E')
         noCarrierState = NO_CARRIER_R3;
      else
         noCarrierState = NO_CARRIER_N;
      break;
   case NO_CARRIER_R3:
      noCarrierState = NO_CARRIER_N;
      if(ch == 'R')
      {
         CommunicationsDisconnect();
         return 1;
      }
      break;
   }
   return 0;
}

/*
** Dispatch the command to the handler
*/
static void DispatchCommand(char command, short cmdLength, char *buff)
{
#ifdef RECIEVE_DEBUG
   printf("\n\nCommand: %d\n", command);
   printf("cmdLength: %d\n", cmdLength);
#endif
   switch(command)
   {
      // Set the IO configuration
   case ConfigurationSetCmd:
      // Set the result buffer size
      InitResult(0, command);

      SetConfiguration(buff);
      break;

      // Motor commands
   case MotorsSetAllCmd:
      // Set the result buffer size
      InitResult(0, command);

      SetAllMotors(buff);
      break;

      // Get the input values
   case GetAllInputCmd:
      // Set the result buffer size
      InitResult(2 * NUM_IO_PORTS // Digital IO/Analog ports
               + 4 * NUM_INTERUPTS, // Sonar & Encoders
               command);

      GetAllInput(buff);
      break;

      // Set the output values
   case SetAllPortsCmd:
      // Set the result buffer size
      InitResult(0, command);

      SetAllPorts(buff);
      break;

      // Preset the encoder value
   case EncoderPresetCmd:
      // Set the result buffer size
      InitResult(0, command);

      EncoderPreset(buff);
      break;

   default:
      // Set the result buffer size
      InitResult(0, command);

      break;

   }
   SendResult();
}

//
// Process the byte stream to get a command
//
void ProcessCommand()
{
   static enum CommandState
   {
      PREAMBLE_1,
      PREAMBLE_2,
      CHECKSUM,
      DATA_LENGTH,
      COMMAND,
      DATA_BLOCK,
   } cmdState = PREAMBLE_1;
   static char     buff[MAX_DATA_BLOCK_SIZE];
   static short    buffOffset = 0;
   static short    cmdLength = 0;
   static char     command;
   static char     checksum;
   unsigned char   count;
   unsigned char   ch;
   static unsigned long startTime;

   //
   // See if we have waited too long for a packet
   //
   // Time = (MAX_DATA_BLOCK_SIZE + 7) / (9,600 bps / 10 bits/byte)
   //      ~ 74mS
   // Allow 2 times that (I'll never notice that) and reset
   //
#ifdef TIMER
   if(cmdState != PREAMBLE_1)    // Are we within a data packet?
   {
      if(GetTimer(1) - startTime > 150) // Have we waited too long?
      {
         cmdState = PREAMBLE_1;
         CommunicationsTimeout();
      }
   }
#endif

   // Get the next byte from the stream
   if(0 == COUNT_PC_PORT())   // VT_20070705_PORT
   {
      return;
   }
   ch = READ_PC_PORT(); // VT_20070705_PORT

   // Make sure we didn't lose the carrier
   if(NoCarrierCheck(ch))
   {
      cmdState = PREAMBLE_1;
      return;
   }

   // See if we connected
   if(ConnectCheck(ch))
   {
      cmdState = PREAMBLE_1;
      return;
   }

#ifdef RECIEVE_BYTES_DEBUG
   printf("state: %d %02x %c\n", cmdState, ch, ch);
#endif

   // Assembled the packet
   switch(cmdState)
   {
      // Look for the preamble - two '$' signs as the signature
   case PREAMBLE_1:
      // Set the buffer offset
      buffOffset = 0;

#ifdef TIMER
      // Reset the timer and get the current time (Should be 0)
      PresetTimer(1, 0);
      startTime = GetTimer(1);
#endif
      // See about transitioning to the next state
      if(ch == '$')
         cmdState = PREAMBLE_2;
      break;

   case PREAMBLE_2:
#ifdef RECIEVE_BYTES_DEBUG
      printf("Preamble\n");
#endif
      if(ch != '$')
         break;
      cmdState = CHECKSUM;
      break;

      // Checksum
   case CHECKSUM:
      checksum = ch;
      cmdState = DATA_LENGTH;
      break;

   // Get the command length
   case DATA_LENGTH:
#ifdef RECIEVE_BYTES_DEBUG
      printf("Length lo\n");
#endif
      cmdLength = ch;
      checksum ^= ch;
      cmdState = COMMAND;
      break;

      // Command
   case COMMAND:
      command = ch;
      checksum ^= ch;
      cmdState = DATA_BLOCK;
#ifdef RECIEVE_BYTES_DEBUG
      printf("cmd: %d (%d)\n", command, cmdLength);
#endif
      // If no data...
      if(0 == cmdLength)
      {
         // And the checksum is ok
         if(0 == checksum)
         {
#ifdef RECIEVE_BYTES_DEBUG
            printf("Dispatching\n");
#endif
            // Dispatch the command
            DispatchCommand(command, cmdLength, buff);
#ifdef RECIEVE_BYTES_DEBUG
            printf("Resetting\n");
#endif
         }
         else
         {
            // Resend command
            InitResult(0, -command);
            SendResult();
         }
         cmdState = PREAMBLE_1;
      }
      break;

      // Load the data block.
   case DATA_BLOCK:
      buff[buffOffset++] = ch;
      checksum ^= ch;

      // Make sure we don't overflow the buffer and corrupt the system
      if(buffOffset >= sizeof(buff))
      {
         buffOffset = sizeof(buff) - 1;
      }

      // See if we've received all of the data packet
      cmdLength--;
      if(0 == cmdLength)
      {
         // Is the checksum correct?
         if(0 == checksum)
         {
#ifdef RECIEVE_BYTES_DEBUG
            printf("Dispatching\n");
#endif
#ifdef RECIEVE_DEBUG
#ifdef TIMER
            printf("Time: %d\n", GetTimer(1));
#endif
#endif
            // Dispatch the command
            DispatchCommand(command, cmdLength, buff);
#ifdef RECIEVE_BYTES_DEBUG
            printf("Resetting\n");
#endif
         }
         else
         {
            // Resend command
            InitResult(0, -command);
            SendResult();
         }
         cmdState = PREAMBLE_1;
      }
      break;
   }
}
